﻿using System;
using System.Collections.Generic;
using Allegro.MathInterface;

namespace Allegro.Mathlib
{
    public class FunctionOptimization
    {
        private const double GOLD = 1.618034;
        private const double GLIMIT = 100.0;
        private const double TINY = 1.0e-20;

        public void BracketMinimum(Function func, ref double ax, ref double bx, out double cx, out double fa, out double fb, out double fc)
        {
            double ulim, u, r, q, fu, dum = 0.0;

            fa = func(ax);
            fb = func(bx);
            if (fb > fa)
            {
                _Shift(ref dum, ref ax, ref bx, ref dum);
                _Shift(ref dum, ref fb, ref fa, ref dum);
            }
            cx = bx + GOLD * (bx - ax); // First guess for c
            fc = func(cx);
            while (fb > fc) // Keep returning here until we bracket
            {
                // Compute u by parabolic extrapolation from a, b, c. TINY is used to prevent any possible division by zero
                r = (bx - ax) * (fb - fc);
                q = (bx - cx) * (fb - fa);
                u = bx - ((bx - cx) * q - (bx - ax) * r) / (2.0 * Math.Sign(q - r) * Math.Max(Math.Abs(q - r), TINY));
                ulim = bx + GLIMIT * (cx - bx);
                // we won't go arther than this. Test various possibilities:
                if ((bx - u) * (u - cx) > 0.0) // parabolic u is between b and c: try it
                {
                    fu = func(u);
                    if (fu < fc) // Got a minimum between b and c
                    {
                        ax = bx;
                        bx = u;
                        fa = fb;
                        fb = fu;
                        return;
                    }
                    else if (fu > fb) // Got a minimum between a and u
                    {
                        cx = u;
                        fc = fu;
                        return;
                    }
                    u = cx + GOLD * (cx - bx); // Parabolic fit was no use. Use default magnification
                    fu = func(u);
                }
                else if ((cx - u) * (u - ulim) > 0.0) // Parabolic fit is between c and its allowed limit
                {
                    fu = func(u);
                    if (fu < fc)
                    {
                        double temp = cx + GOLD * (cx - bx);
                        _Shift(ref bx, ref cx, ref u, ref temp);
                        temp = func(u);
                        _Shift(ref fb, ref fc, ref fu, ref temp);
                    }
                }
                else if ((u - ulim) * (ulim - cx) >= 0.0) // Limit parabolic u to max. allowed value
                {
                    u = ulim;
                    fu = func(u);
                }
                else // reject parabolic u, use default magnification
                {
                    u = cx + GOLD*(cx - bx);
                    fu = func(u);
                }
                _Shift(ref ax, ref bx, ref cx, ref u); // Eliminate oldest point and continue
                _Shift(ref fa, ref fb, ref fc, ref fu);
            }
        }


        protected static void _Shift(ref double a, ref double b, ref double c, ref double d)
        {
            a = b;
            b = c;
            c = d;
        }
        protected static void _Move(ref double a, ref double b, ref double c, ref double d, ref double e, ref double f)
        {
            a = d;
            b = e;
            c = f;
        }
    }
}
